package org.amphiprion.gameengine.util;

import org.amphiprion.gameengine.ScreenProperty;

import android.opengl.GLU;
import android.opengl.Matrix;

public class MathUtil {

	public static double[] toEuler(double x, double y, double z, double angle) {
		double[] result = new double[3];
		double s = Math.sin(angle);
		double c = Math.cos(angle);
		double t = 1 - c;
		// if axis is not already normalised then uncomment this
		// double magnitude = Math.sqrt(x*x + y*y + z*z);
		// if (magnitude==0) throw error;
		// x /= magnitude;
		// y /= magnitude;
		// z /= magnitude;
		double heading = 0;
		double attitude = 0;
		double bank = 0;
		if (x * y * t + z * s > 0.998) { // north pole singularity detected
			heading = 2 * Math.atan2(x * Math.sin(angle / 2), Math.cos(angle / 2));
			attitude = Math.PI / 2;
			bank = 0;

		} else if (x * y * t + z * s < -0.998) { // south pole singularity detected
			heading = -2 * Math.atan2(x * Math.sin(angle / 2), Math.cos(angle / 2));
			attitude = -Math.PI / 2;
			bank = 0;

		} else {
			heading = Math.atan2(y * s - x * z * t, 1 - (y * y + z * z) * t);
			attitude = Math.asin(x * y * t + z * s);
			bank = Math.atan2(x * s - y * z * t, 1 - (x * x + z * z) * t);
		}
		result[0] = bank * 180 / Math.PI;
		result[1] = attitude * 180 / Math.PI;
		result[2] = heading * 180 / Math.PI;

		return result;
	}

	public static float[] ScreenTo3D(float screenX, float screenY, ScreenProperty screenProperty, float targetZ) {
		float[] posCamera = screenProperty.camera.getEye();

		// int[] viewport = { 0, 0, (int) screenProperty.realWidth, (int) screenProperty.realHeight };
		// float[] nearCoord = new float[4];
		// float[] farCoord = new float[4];
		// float[] ray = new float[4];
		// GLU.gluUnProject(screenX, screenProperty.realHeight - screenY, 1, screenProperty.viewMatrixFor3D, 0, screenProperty.projectionMatrixFor3D, 0, viewport, 0, ray, 0);
		// Matrix.multiplyMV(nearCoord, 0, screenProperty.viewMatrixFor3D, 0, ray, 0);
		// nearCoord[0] = nearCoord[0] / nearCoord[3];
		// nearCoord[1] = nearCoord[1] / nearCoord[3];
		// nearCoord[2] = nearCoord[2] / nearCoord[3];
		//
		// GLU.gluUnProject(screenX, screenY, 0, screenProperty.viewMatrixFor3D, 0, screenProperty.projectionMatrixFor3D, 0, viewport, 0, ray, 0);
		// Matrix.multiplyMV(farCoord, 0, screenProperty.viewMatrixFor3D, 0, ray, 0);
		// farCoord[0] = farCoord[0] / farCoord[3];
		// farCoord[1] = farCoord[1] / farCoord[3];
		// farCoord[2] = farCoord[2] / farCoord[3];
		//
		// ray[0] = nearCoord[0] - farCoord[0];
		// ray[1] = nearCoord[1] - farCoord[1];
		// ray[2] = nearCoord[2] - farCoord[2];

		// Log.d(ApplicationConstants.PACKAGE, "   GLU x = " + obj[0] + "  y=" + obj[1] + "  z=" + obj[2]);

		float[] ray = getViewRay(screenX, screenY, screenProperty);

		float mult = (-posCamera[2] + targetZ) / ray[2];
		// Log.d("OPENGL", "3D board x=" + (posCamera[0] + ray[0] * mult) +
		// "  y=" + (posCamera[1] + ray[1] * mult) + " z=" + (posCamera[2] +
		// ray[2] * mult));
		return new float[] { posCamera[0] + ray[0] * mult, posCamera[1] + ray[1] * mult, posCamera[2] + ray[2] * mult };
	}

	// public static double getDistance(float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) {
	// return Math.Sqrt((p1x - p2x) * (p1x - p2x) + (p1y - p2y) * (p1y - p2y) + (p1z - p2z) * (p1z - p2z));
	// }
	//
	// public static double getDistance(float[] p1, float[] p2) {
	// return getDistance(p1[0], p1[1], p1[2], p2[0], p2[1], p2[2]);
	// }
	//
	// public static float[] ScreenTo3D(float screenX, float screenY, float screenWidth, float screenHeight, float[] posCamera, float targetZ) {
	// float[] ray = getViewRay(screenX, screenY, screenWidth, screenHeight, posCamera);
	// float mult = (-posCamera[2] + targetZ) / ray[2];
	// // Log.d("OPENGL", "3D board x=" + (posCamera[0] + ray[0] * mult) +
	// // "  y=" + (posCamera[1] + ray[1] * mult) + " z=" + (posCamera[2] +
	// // ray[2] * mult));
	// return new float[] { (posCamera[0] + ray[0] * mult), (posCamera[1] + ray[1] * mult), (posCamera[2] + ray[2] * mult) };
	// }
	//
	public static float[] getViewRay(float screenX, float screenY, ScreenProperty screenProperty) {

		int[] viewport = { 0, 0, (int) screenProperty.realWidth, (int) screenProperty.realHeight };
		float[] nearCoord = new float[4];
		float[] farCoord = new float[4];
		float[] ray = new float[4];
		GLU.gluUnProject(screenX, screenProperty.realHeight - screenY, 1, screenProperty.viewMatrixFor3D, 0, screenProperty.projectionMatrixFor3D, 0, viewport, 0, ray, 0);
		Matrix.multiplyMV(nearCoord, 0, screenProperty.viewMatrixFor3D, 0, ray, 0);
		nearCoord[0] = nearCoord[0] / nearCoord[3];
		nearCoord[1] = nearCoord[1] / nearCoord[3];
		nearCoord[2] = nearCoord[2] / nearCoord[3];

		GLU.gluUnProject(screenX, screenY, 0, screenProperty.viewMatrixFor3D, 0, screenProperty.projectionMatrixFor3D, 0, viewport, 0, ray, 0);
		Matrix.multiplyMV(farCoord, 0, screenProperty.viewMatrixFor3D, 0, ray, 0);
		farCoord[0] = farCoord[0] / farCoord[3];
		farCoord[1] = farCoord[1] / farCoord[3];
		farCoord[2] = farCoord[2] / farCoord[3];

		ray[0] = nearCoord[0] - farCoord[0];
		ray[1] = nearCoord[1] - farCoord[1];
		ray[2] = nearCoord[2] - farCoord[2];

		return ray;
	}

	// }

}
